#include "TypesCommands.h"
#include "Types.h"
#include "CButtonHandler.h"
#include "CTimerHandler.h"
#include "CCommand.h"
#include "AFMotor.h"

// Handles button. If you do not connect button to the device, it just does nothing
//CButtonHandler button1(2);          // Switch button 1 is connected to pin 2

// This timer would be used for Watchdog implementation in SerialSocketBridge app
CTimerHandler  timerOnline(5000);   // ARDUINO_ONLINE command is sent every 5 seconds
const int ENGINE_WORK_TIME = 500;  // Engine works for 1 second after each command
const int ENGINE_SPEED = 168;

// Using library AFMotor for motors handling. Probably Motors should be reassigned to other channels of Motor shield.
AF_DCMotor motor[] = {AF_DCMotor(ENGINE_2/*, MOTOR12_64KHZ*/), AF_DCMotor(ENGINE_1/*, MOTOR12_64KHZ*/)};

// These timers are used to implement ENGINE_WORK_TIME logic (see comment above)
CTimerHandler engineTimers[] = {CTimerHandler(ENGINE_WORK_TIME), CTimerHandler(ENGINE_WORK_TIME), CTimerHandler(ENGINE_WORK_TIME), CTimerHandler(ENGINE_WORK_TIME)};


//
// Methods Forward declarations
void controlEngine(eEngineName engineName, eEngineDirection engineDirection, uint8_t speed = 255);
void moveForward(uint8_t speed = ENGINE_SPEED);
void moveBackward(uint8_t speed = ENGINE_SPEED);
void moveRight(uint8_t speed = ENGINE_SPEED);
void moveLeft(uint8_t speed = ENGINE_SPEED);

//Motor4 - right
//Motor2 - left

void setup()
{
  Serial.begin(9600);

  // init of Motors. Initial state is Off
  motor[0].run(RELEASE);
  motor[1].run(RELEASE);

  // init of button
//  button1.init();
  
  // init if Watchdog timer
  timerOnline.start();
}

void loop()
{
  // ===========================================================
  // BUTTONS HANDLING SECTION
  //onButton1Event(button1.processButton());

  // ===========================================================
  // TIMERS HANDLING SECTION  
  // Timers handling
  if (TIMER_FIRED == timerOnline.processTimer())
  {
     onTimerOnlineEvent();
  }

  // Processing event for each motor - when there is no more commands to run motor within timeout - stopping motor
  for (int i=0; i<2; i++)
  {
    if (TIMER_FIRED == engineTimers[i].processTimer())
    {
      motor[i].setSpeed(0);
      engineTimers[i].stop();
    }
  }

  // ===========================================================
  // COMMANDS HANDLING SECTION
  CCommand incomingCommand = readCommand();

  if (COMMAND_PC_ENGINE_FORWARD == incomingCommand.mCommandType)
  {
    moveForward();
  }
  else if (COMMAND_PC_ENGINE_BACKWARD == incomingCommand.mCommandType)
  {
    moveBackward();
  }
  else if (COMMAND_PC_ENGINE_LEFT == incomingCommand.mCommandType)
  {
    moveLeft();
  }
  else if (COMMAND_PC_ENGINE_RIGHT == incomingCommand.mCommandType)
  {
    moveRight();
  }
}

/**
 * @brief Reads command from Serial port
 * @return Command object if command was successfully read. OR Command object with COMMAND_INVALID if nothing read
 **/
CCommand readCommand()
{
  if (Serial.available() >= 5)  // magic number 5 - length of command
  {
    unsigned char cmd[5];
    for (int i=0; i<5; i++)
    {
      cmd[i] = Serial.read();
    }
    
    return CCommand(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4]);
  }
  else
  {
    return CCommand(COMMAND_INVALID, 0,0,0,0);
  }
}

/**
 * @brief Handling button events
 */
void onButton1Event(uint8_t btnValue)
{
   if (BUTTON_PRESS == btnValue)
   {
      // pressed 
      CCommand cmd(COMMAND_ARDUINO_BTNSW_1, 0,0,0, BUTTON_PRESS);
      cmd.send();
      
   } else if (BUTTON_RELEASE == btnValue)
   {
      // released
      CCommand cmd(COMMAND_ARDUINO_BTNSW_1, 0,0,0, BUTTON_RELEASE);
      cmd.send();
   }
}

/**
 * @brief Handling ONLINE timer event
 * sends COMMAND_ARDUINO_ONLINE command to serial port
 */
void onTimerOnlineEvent()
{
  static int valueCount = 0;
  CCommand cmd(COMMAND_ARDUINO_ONLINE, valueCount+3, valueCount+2, valueCount+1, valueCount);
  cmd.send();
  valueCount++;
}

/**
 * @brief Controls motor
 * Wrapper over AFMotor library
 */
void controlEngine(uint8_t engineName, eEngineDirection engineDirection, uint8_t speed)
{
  if (speed > 0)
  {
    motor[engineName].setSpeed(speed);
    if (engineDirection == ENGINE_DIRECTION_BACKWARD)
    {
      motor[engineName].run(BACKWARD);
      motor[engineName].setSpeed(speed);
    }
    else
    {
      motor[engineName].run(FORWARD);
      motor[engineName].setSpeed(speed);
    }
    
    // Start timer that will turn off engine after N seconds (if next command comes, engine would be working non-stop)
    engineTimers[engineName].start();
  }
  else
  {
    motor[engineName].run(RELEASE);
  }
}

void moveForward(uint8_t speed)
{
  controlEngine(0, ENGINE_DIRECTION_FORWARD, speed);
  controlEngine(1, ENGINE_DIRECTION_FORWARD, speed);
}

void moveBackward(uint8_t speed)
{
  controlEngine(0, ENGINE_DIRECTION_BACKWARD, speed);
  controlEngine(1, ENGINE_DIRECTION_BACKWARD, speed);
}

void moveLeft(uint8_t speed)
{
  controlEngine(0, ENGINE_DIRECTION_FORWARD, speed);
  controlEngine(1, ENGINE_DIRECTION_BACKWARD, speed);
}

void moveRight(uint8_t speed)
{
  controlEngine(1, ENGINE_DIRECTION_FORWARD, speed);
  controlEngine(0, ENGINE_DIRECTION_BACKWARD, speed);
}

